/*********************************************************************
*
*   RobotMath.cpp - Robot math module
*
*   Copyright:
*       Software source code by Alex Morozov and Chris D. Locke is
*       licensed under a Creative Commons Attribution-Noncommercial-
*       Share Alike 3.0 United States License
*       (http://creativecommons.org/licenses/by-nc-sa/3.0/us/)
*
*********************************************************************/
#include <stdio.h>
#include <math.h>

#include "RobotMath.h"

/******************************************************************************
* Compute robot's rotation angle.
* Output angle is measured clockwise from the robot's yaw angle.
******************************************************************************/
void ComputeRotationAngle( double leftWheelSpeed,
                           double rightWheelSpeed,
                           double robotWidth,
                           double * angle,
                           double * turnRadius )
{
// Initialize output before checking for errors
*angle = 0.0;
*turnRadius = 0.0;

// Robot width should not be zero
if( fabs( robotWidth ) < EPSILON )
    {
    printf( "Warning: robot is too narrow (%f)\n", robotWidth );
    return;
    }

// Wheels should be turning at different speeds
double speedDifference = leftWheelSpeed - rightWheelSpeed;
if( fabs( speedDifference ) < WHEEL_SPEED_EPSILON )
    {
    printf( "Warning: don't call ComputeRotationAngle() if wheels are turning "
            "at the same speed (left=%f, right=%f)\n",
            leftWheelSpeed, rightWheelSpeed );
    return;
    }

// Assume that wheels are turning at different speeds.
// Otherwise this function should not have been called.

// If wheels are turning in opposite directions
if ( ( leftWheelSpeed >= 0 ) != ( rightWheelSpeed >= 0 ) )
    {
    *turnRadius = ( leftWheelSpeed + rightWheelSpeed ) * robotWidth / 2;
    if( rightWheelSpeed >= leftWheelSpeed )
        {
        *turnRadius = -*turnRadius;
        }

    *angle = speedDifference / ( robotWidth / 2 + fabs( *turnRadius ) );
    }

// If both wheels are turning in the same direction...
else
    {
    double fastWheelSpeed, slowWheelSpeed, radiusSign;
    if( fabs( leftWheelSpeed ) >= fabs( rightWheelSpeed ) )
        {
        fastWheelSpeed = leftWheelSpeed;
        slowWheelSpeed = rightWheelSpeed;
        radiusSign = 1.0;
        }
    else
        {
        fastWheelSpeed = rightWheelSpeed;
        slowWheelSpeed = leftWheelSpeed;
        radiusSign = -1.0;
        }

    double angleSign = ( fastWheelSpeed >= 0 ) ? 1.0 : -1.0;
    *angle = fabs( speedDifference ) / robotWidth;
    *turnRadius = radiusSign * ( fabs( slowWheelSpeed ) / *angle + 0.5 * robotWidth );
    *angle = *angle * angleSign * radiusSign;
    }
}


/******************************************************************************
* Convert yaw (measured clockwise from the North) into a geometric angle
* (measured counter-clockwise from the x-axis)
******************************************************************************/
double ConvertYawToTheta( double yaw )
{
    return( PI / 2.0 - yaw );
}


/******************************************************************************
* Rotate (x,y) clockwise about (xo,yo).
******************************************************************************/
void RotateAboutPoint( double * x, double * y, double xo, double yo, double angle )
{
    // Local variables
    double localX;                  // local copy of x
    double localY;                  // local copy of y
    double rotatedX;                    // x after rotation
    double rotatedY;                    // y after rotation

    // Initialize local variables
    localX = *x;
    localY = *y;

    // Translate (x,y) into coordiante system centered at (xo.yo)
    localX = localX - xo;
    localY = localY - yo;

    // Roate
    rotatedX = localX * cos( angle ) + localY * sin( angle );
    rotatedY = -localX * sin( angle ) + localY * cos( angle );

    // Translate back into the original coordinate system
    *x = rotatedX + xo;
    *y = rotatedY + yo;
}
